#include <gcode/move/G0_G1.h>
/*
 * Copyright (c) Halo Team
 *
 * SPDX-License-Identifier: GPLv3
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-10-31     Eywen       the first version
 */

#include "../../common.h"
#include ".././config_print.h"

/* 保存最后一次的gcode运动的目标位置 */
static motion_t s_last_gcode_parse = {
    .speed = 1500,
#if IS_ENABLE(CONFIG_LASER)
    .laser_power = 0.000,
#endif

#if IS_ENABLE(CONFIG_EXTRUDER)
    .feed_rate = 0.000,
#endif
    .x = 0.000,
    .y = 0.000,
    .z = 0.000,
    .e = 0.000,
};

/* 上层处理gcode命令 g0 g1命令 */
bool gcode_handle_G0_G1(gcode_t *gcode, bool instantly, fdm_printer_t *fdm_printer)
{
    ASSERT_NULL(gcode);
    ASSERT_NULL(fdm_printer);

    gcode_para_t *para;
    for(int i = 0; i < gcode->para_cnt; i++){
        para = &(gcode->para[i]);
        switch (para->head){
        case 'X':
            s_last_gcode_parse.x = str_to_float(para->number);
            break;
        case 'Y':
            s_last_gcode_parse.y = str_to_float(para->number);
            break;
        case 'Z':
            s_last_gcode_parse.z = str_to_float(para->number);
            break;
        case 'E':
            s_last_gcode_parse.e = str_to_float(para->number);
            break;
#if IS_ENABLE(CONFIG_LASER)
        case 'S':
            s_last_gcode_parse.laser_power = str_to_float(para->number);
            break;
#endif
#if IS_ENABLE(CONFIG_EXTRUDER)
        case 'F':
            s_last_gcode_parse.speed = str_to_float(para->number);
            break;
#endif
        }
    }
    /* 立即运行 */
    if(instantly == true){
        /* 添加运动任务到运动线程 */
#if IS_ENABLE(USING_RTTHREAD_OS)
        //fdm_printer->gcode_handle->ops->add_motion_task(fdm_printer->gcode_handle, &s_last_gcode_parse);
        /* 将新的目标位置添加到 motion_control 对象的 gcode_motion_list*/
        while(fdm_printer->motion_control->ops->add_gcode_task(fdm_printer->motion_control,
                &s_last_gcode_parse) == false){
            //debug_info("gcode task buff full");
            rt_thread_yield();
        }
        //fdm_printer->motion_control->ops->add_vector();
#else
        /* 暂不处理，后期添加逻辑 */
        /* 手动将新的运动目标位置添加到运动任务队列 */
        fdm_printer->motion_control->ops->add_gcode_task(fdm_printer->motion_control, &s_last_gcode_parse);
#endif
    }
    /* 添加到队列后等待被运行 */
    else{
        while(fdm_printer->motion_control->ops->add_gcode_task(fdm_printer->motion_control,
                &s_last_gcode_parse) == false){
            //debug_info("gcode task buff full");
            rt_thread_yield();
        }
    }
//    debug_info("gcode motion list used: %d",
//            ringbuff_used_count(fdm_printer->motion_control->gcode_motion_list));

    return true;
}
